#include <public/sgi_link_layer.h>
#include <public/globalsensor_tc_store.h>
#include <public/ssi_link_layer.h>
#include <iostream>
#include <cstdlib>

using namespace std;

#define PACK_BYTES_FOR_STORING_TC_FRAMES 254
#define MAX_SENSOR_TC_PACK_LIST 225
#define MAX_BYTES_FROM_FILE 30
#define MIN_BYTES_FROM_FILE 1
#define MAX_SENSOR 2


void CGlobalSensorTCStore::add_free_pack_list(CPackListHandler &packList){
	packList.ExtractAllPackages(m_freePackList);
}

void CGlobalSensorTCStore::get_free_pack_list(CPackListHandler &packList){
	m_freePackList.ExtractAllPackages(packList);
}

uint16_t CGlobalSensorTCStore::get_free_pack_list_number(){

	return m_freePackList.GetPackCounter();
}

int16_t CGlobalSensorTCStore::check_next_packed_sensor_tc(int16_t sensor_id){
	//Busca si el Sensor_id está en la lista de sensores
	//Retorna >=0 si existe un TC para este sensor y es valido
	//Retorna -1 si no existe
	int16_t index=-1;
	for(int16_t i=0 ;i < (MAX_SENSOR_TC_PACK_LIST + 1); i++){
		if (!m_sensorTCPackList[i].IsEmpty()){
			if (m_sensorID[i]==sensor_id){
				index=i;
			}
		}
	}
	return index;
}


bool CGlobalSensorTCStore::get_next_ssi_sen_tc_frame_info(int16_t sensor_id,struct ssi_link_layer_data_frame_info_t &frame_info){

	CPackListHandler free_pack_list;

	int16_t index;
	index=sensor_id;
	CPackHandler currentPack;
	uint8_t FTO;
	uint8_t length;
	uint8_t id_sensor;
	//uint8_t LTO;


	if (index>=0){
		if ((m_sensorTCPackList[index].GetPackCounter())==0){return false;}
		m_sensorTCPackList[index].ToHeadPack();
		m_sensorTCPackList[index].ReadByte(1,FTO);//Para saber donde comienza el primer TC
		m_sensorTCPackList[index].ReadByte(FTO,length);//Para leer el primer byte del TC que contiene la longitud
		frame_info.length=length;
		FTO++;
		m_sensorTCPackList[index].ReadByte(FTO,id_sensor);//leer sensor_id
		frame_info.sensor_id=id_sensor;

		int i=0;
		int8_t pos= FTO+2;//antes era +3
		while(length){
			m_sensorTCPackList[index].ReadByte(pos,frame_info.data[i]);
			pos++;
			i++;
			length--;
		}
		return true;
	}else
		return false;
}


bool CGlobalSensorTCStore::delete_packed_tc_head(uint16_t sensor_id){

	uint16_t index;
	//index=check_next_packed_sensor_tc(sensor_id);
	index=sensor_id;
	uint8_t FTO;
	uint8_t NT;
	uint8_t length;

	CPackHandler m_currentPack;


		m_sensorTCPackList[index].ToHeadPack();
		m_sensorTCPackList[index].ReadByte(0,NT);
		m_sensorTCPackList[index].ReadByte(1,FTO);


		if (NT==1){
			m_sensorTCPackList[index].TakeOnePack(m_currentPack); //pasarlo a la lista de free
			m_freePackList.AddPack(m_currentPack);

		} else if (NT>1){
			NT--;
			m_sensorTCPackList[index].ReadByte(FTO,length);
			FTO= FTO+length+5;
			m_sensorTCPackList[index].WriteByte(1,FTO);
			m_sensorTCPackList[index].WriteByte(0,NT);
		}else
        return false;


}

int16_t CGlobalSensorTCStore::get_free_sensor_tc_list_index(){
	uint16_t index=0; //inicializado recientemente
	for(int16_t i=0 ;i < (MAX_SENSOR_TC_PACK_LIST + 1); i++){
		if (m_sensorTCPackList[i].IsEmpty()){
			index=i;
			return index;
		}
	}

	return index;
}

bool CGlobalSensorTCStore::store_sen_tc_frame(struct sgi_link_layer_sen_tc_frame_t  &sen_tc){

	bool tc_frame_stored=true;
	int16_t index;
	index=check_next_packed_sensor_tc(sen_tc.sensor_id);

	if(index==-1){ //solo se mete 1 vez, la primera que crea la lista de paquetes

		CPackHandler currentPack;
		uint8_t aux=3;
		byte_t LTO;
		//int8_t NT=1;
		index=get_free_sensor_tc_list_index();
		m_sensorID[index]=sen_tc.sensor_id;
	    m_freePackList.TakeOnePack(currentPack);

		m_sensorTCPackList[index].AddPack(currentPack);
		m_sensorTCPackList[index].GetPackCounter();

		m_sensorTCPackList[index].ToTailPack();
		m_sensorTCPackList[index].WriteByte(0,1); //NT=1

		m_sensorTCPackList[index].WriteByte(1,3); //FTO=3;
		m_sensorTCPackList[index].WriteByte(2,3); //LTO=3;
		m_sensorTCPackList[index].WriteByte(aux,sen_tc.length);
		aux++;

		m_sensorTCPackList[index].WriteByte(aux,sen_tc.sensor_id);
		aux++;
		aux++;

		for(int i=0; i<sen_tc.length; i++){
			m_sensorTCPackList[index].WriteByte(aux,sen_tc.data[i]);
			aux++;
		}
		aux++;//2 bytes mas para el crc
		aux++;
		LTO=LastTCOfFrame(index,sen_tc);
		m_sensorTCPackList[index].WriteByte(2,LTO);


	}
	else{

		//CPackHandler currentPack;
		uint8_t LTO;
		uint8_t NT;
		uint8_t FTO;
		if(index!=-1){
			m_sensorTCPackList[index].ReadByte(1,FTO);
			m_sensorTCPackList[index].ReadByte(2,LTO);
			if((sen_tc.length+3)<(PACK_BYTES_FOR_STORING_TC_FRAMES-LTO)){//Aqui habia +5
				m_sensorTCPackList[index].WriteByte(LTO,sen_tc.length);
				LTO++;
				m_sensorTCPackList[index].WriteByte(LTO,sen_tc.sensor_id);
				LTO++;
				LTO++;
				for(int i=0; i<sen_tc.length; i++){
					m_sensorTCPackList[index].WriteByte(LTO,sen_tc.data[i]);
					LTO++;
				}
				LTO++;//2 bytes mas para el crc
				LTO++;


				m_sensorTCPackList[index].ReadByte(0,NT);
				NT++;
				m_sensorTCPackList[index].WriteByte(0,NT); //NT=0
				m_sensorTCPackList[index].WriteByte(2,LTO);

			}else{
				while( LTO < (PACK_BYTES_FOR_STORING_TC_FRAMES)){
					m_sensorTCPackList[index].WriteByte(LTO,0);
					LTO++;

				}
				CPackHandler currentPack;
				uint8_t aux=3;
				uint8_t NT;
				NT=0;
				m_freePackList.TakeOnePack(currentPack);
				m_sensorTCPackList[index].AddPack(currentPack);
				m_sensorTCPackList[index].ToTailPack();
				m_sensorTCPackList[index].WriteByte(0,0); //NT=0
				m_sensorTCPackList[index].WriteByte(1,3); //FTO=2;
				m_sensorTCPackList[index].WriteByte(2,3); //LTO=3;
				m_sensorTCPackList[index].WriteByte(aux,sen_tc.length);
				aux++;

				m_sensorTCPackList[index].WriteByte(aux,sen_tc.sensor_id);
				aux++;
				aux++;

				for(int i=0; i<sen_tc.length; i++){
					m_sensorTCPackList[index].WriteByte(aux,sen_tc.data[i]);
					aux++;
				}
				aux++;//2 bytes mas para el crc
				aux++;
				m_sensorTCPackList[index].ReadByte(0,NT);
				NT++;
				m_sensorTCPackList[index].WriteByte(0,NT);


				LTO=LastTCOfFrame(index,sen_tc);
				m_sensorTCPackList[index].WriteByte(2,LTO);
			}
		}

		else tc_frame_stored=false;
	}

	return tc_frame_stored;
}

int8_t CGlobalSensorTCStore::LastTCOfFrame(uint8_t index, sgi_link_layer_sen_tc_frame_t  &sen_tc){

	uint8_t FTO;
	uint8_t NT;
	//uint8_t length;
	uint8_t LTO;
	ssi_link_layer_data_frame_info_t ssi_tc;
	m_sensorTCPackList[index].ReadByte(0,NT);
	while (NT){
		m_sensorTCPackList[index].ReadByte(1,FTO);
		m_sensorTCPackList[index].ReadByte(2,LTO);
		LTO=LTO+sen_tc.length+5;
		NT--;
		m_sensorTCPackList[index].WriteByte(2,LTO);
		if (NT!=0){
			get_next_ssi_sen_tc_frame_info(sen_tc.sensor_id,ssi_tc);
		}

	}
	return LTO;
}


